#include <iostream>
#include <opencv4/opencv2/opencv.hpp>
#include <opencv4/opencv2/core/core.hpp>
#include <opencv4/opencv2/highgui/highgui.hpp>
#include <opencv4/opencv2/features2d/features2d.hpp>
#include <vector>

using namespace std;
using namespace cv;

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoint_1,
                          std::vector<KeyPoint> &keypoint_2,
                          std::vector<DMatch> &matches);

void pose_estimation_2d2d(const std::vector<KeyPoint> &keypoint_1,
                          const std::vector<KeyPoint> &keypoint_2,
                          const std::vector<DMatch> &matches,
                          Mat &R, Mat &t);

void triangulation(const vector<KeyPoint> &keypoint_1,
                   const vector<KeyPoint> &keypoint_2,
                   const std::vector<DMatch> &matches,
                   const Mat &R, const Mat &t,
                   vector<Point3d> &points
     );  //三角化的目的是求取点的深度信息

inline cv::Scalar get_color(float depth)
{
    float up_th=50,low_th=10, th_range=up_th-low_th;
    if(depth>up_th) depth=up_th;
    if(depth<low_th) depth=low_th;
    return cv::Scalar(255*depth/th_range,0,255*(1-depth/th_range));
}

//像素坐标转换函数
Point2f pixel2cam（const Point2d &p, const Mat &K）;
    
                    
    

void triangulation(const vector<KeyPoint> &keypoint_1,
                   const vector<KeyPoint> &keypoint_2,
                   const std::vector<DMatch> &matches,
                   const Mat &R, const Mat &t, vector<Point3d> &points)
{
    Mat T1=(Mat_<float_(3,4)<<
    1，0，0，0，
    0，1，0，0，
    0，0，1，0
    );
    
    Mat T2=(Mat_<float_(3,4)<<
    R.at<double>(0,0),R.at<double>(0,1),R.at<double>(0,2),R.at<double>(0,0),
    R.at<double>(1,0),R.at<double>(1,1),R.at<double>(1,2),R.at<double>(1,0),
    R.at<double>(2,0),R.at<double>(2,1),R.at<double>(2,2),R.at<double>(2,0),
    );
    
    Mat K=(Mat_<float>(3,4)<<520.9,325.1,0,521.0,249.7,0,0,1);  //内参
    vector<Point2f> pts_1, pts_2;
    
    for(DMatch m:matches)
    {
        //像素坐标转化为相机坐标
        pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt,K));
        pts_2.push_back(pixel2cam(keypoint_2[m.queryIdx].pt,K));
    }
    
    Mat pts_4d;
    cv::triangulatePoint(T1,T2,pts_1,pts_2,pts_4d);
    //转换成非齐次坐标
    for(int i=0;i<pts_4d.cols;i++)
    {
        Mat x=pts_4d.col(i);
        x/=x.at<float>(3,0);
        Point3d p(
            x.at<float>(0,0),
            x.at<float>(1,0),
            x.at<float>(2,0)
        );
        points.push_back(p);
    }
}
        
        
        
        


